function [x, r, zIter, xIter, rIter, dz, ind] = lsq_feedbackresid(H, z, K, nIter)
%LSQ_FEEDBACKRESID 迭代负反馈残差线性最小二乘
%
% Input Arguments
% # H: m*n系数矩阵
% # z: m*1量测向量
% # K: m*m残差反馈系数矩阵，默认的K仅负反馈最大的残差元素值
% # nIter: 标量，迭代次数，默认为10
%
% Output Arguments
% # x: n*1状态向量，迭代计算结果
% # r: m*1向量，各次迭代残差累计值
% # zIter: m*nIter矩阵，各列为各次迭代的量测向量
% # xIter: n*nIter矩阵，各列为各次迭代的状态向量
% # rIter: m*nIter矩阵，各列为各次迭代的残差向量
% # dz: m*1向量，量测向量中的系统误差
% # ind: 标量，首次迭代残差中绝对值最大元素序号

if nargin < 4
    nIter = 10;
end
if nargin < 3
    K = [];
end

[m, n] = size(H);
zIter = NaN(m, nIter);
xIter = NaN(n, nIter);
rIter = NaN(m, nIter);

G = pinv(H);
zIter(:, 1) = z;
xIter(:, 1) = G * z;
rIter(:, 1) = z - H*xIter(:, 1);
if isempty(K)
    [~, ind] = max(abs(rIter(:, 1)));
    d = zeros(m, 1);
    d(ind) = 1;
    K = diag(d);
else
    ind = NaN;
end
for i=1:(nIter-1)
    zIter(:, i+1) = zIter(:, i) - K*rIter(:, i);
    xIter(:, i+1) = G * zIter(:, i+1);
    rIter(:, i+1) = zIter(:, i+1) - H*xIter(:, i+1);
end
x = xIter(:, end);
r = sum(rIter, 2);
dz = K * r;
end